#include "tello_control/Tello.h"

Posture target;

void cb_target(geometry_msgs::PoseStamped::ConstPtr msg)
{
    // cout << "收到目标位置" <<endl;
    target.x = msg->pose.position.x;
    target.y = msg->pose.position.y;
    target.z = msg->pose.position.z;
}

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"tello_target");
    ros::NodeHandle nh;
    ros::Subscriber target_sub;
    target_sub = nh.subscribe("/target",10,cb_target);
    std::vector<Tello* > uavs;
    Tello uav;
    uavs.push_back(&uav);
    uav.setID(1);
    uav.initpublisher(nh);
    uav.initsubscriber(nh);
    ros::Rate loop_rate(50);
    // cout << "你在干什么" <<endl;
    while (ros::ok())
    {
        // cout << "无人机ID为" << uav.getID() << endl;
        cout << "当前无人机位置为:" <<uav.pos.x<<" "<<uav.pos.y<<endl;
        uav.vepul(uavs);
        uav.vel_control1(target,1.0);
        cout << "当前目标为:" << target.x <<" "<< target.y << endl;
        cout << uav.cmd_vel.linear.x << " "<<uav.cmd_vel.linear.y <<endl;
        ros::spinOnce();
	    loop_rate.sleep();
    }
    return 0;
}
